mod vehicle_controller;

use pyo3::prelude::*;
use pyo3::exceptions::PyOSError;
use rppal::i2c::I2c;

const DEVICE_ADDRESS: u16 = 0x2B; // 设备地址
const MOTOR_ADDRESS: u8 = 0x01; // 电机地址
const SERVO_ADDRESS: u8 = 0x02; // 舵机地址
const LED_ADDRESS: u8 = 0x03; // LED 灯的地址
const LED_UNIT_ADDRESS: u8 = 0x04; // LED 灯的单位地址
const MOTOR_FORWARD: u8 = 0x00; // 电机正转
const MOTOR_BACKWARD: u8 = 0x01; // 电机反转
const MOTOR_SPEED: u8 = 50; // 电机默认速度
const ULTRASONIC_SENSOR_ADDRESS: u8 = 0x07; // 超声波传感器地址
const ULTRASONIC_LOW_ADDRESS: u8 = 0x1A; // 超声波传感器低地址
const ULTRASONIC_HIGH_ADDRESS: u8 = 0x1B; // 超声波传感器高地址

#[pyclass]
pub struct Vehicle {
    i2c: I2c,
}

#[pymethods]
impl Vehicle {
    #[new]
    pub fn new() -> PyResult<Self> {
        let mut i2c = I2c::new().map_err(|e| PyOSError::new_err(e.to_string()))?;
        i2c.set_slave_address(DEVICE_ADDRESS)
            .map_err(|e| PyOSError::new_err(e.to_string()))?; // 设置设备地址
        Ok(Vehicle { i2c })
    }
    
    #[pyo3(signature = (motor_id, motor_dir = 0, motor_speed = MOTOR_SPEED as i32))]
    pub fn Ctrl_Car(&mut self, motor_id: u8, motor_dir: i32, motor_speed: i32) -> PyResult<()> {
        // Direction Parameter Check
        let valid_dir = match motor_dir {
            0 | 1 => motor_dir as u8,
            _ => {
                println!("Invalid direction {}, default to 0 (forward)", motor_dir);
                0
            }
        };

        // Speed Parameter Check
        let clamped_speed = motor_speed.clamp(0, 255) as u8;
        
        // Select Control Command Based on Direction
        let command_code = match valid_dir {
            0 => MOTOR_FORWARD,
            1 => MOTOR_BACKWARD
        };


        // Actual Speed: Force speed to 0 when stopping
        let actual_speed = if valid_dir == 0 { 0 } else { clamped_speed };


        let motor_command = [MOTOR_ADDRESS, motor_id, command_code, actual_speed];
        self.i2c.write(&motor_command)
            .map_err(|e| PyOSError::new_err(e.to_string()))?;

        // Add Detailed Status Output

        match valid_dir {
            0 => println!("Motor {} forward, speed: {}", motor_id, actual_speed),
            1 => println!("Motor {} backward, speed: {}", motor_id, actual_speed),
            _ => unreachable!()
        }


        Ok(())
    }

    #[pyo3(signature = (servo_id, angle))]
    pub fn Ctrl_Servo(&mut self, servo_id: u8, angle: i32) -> PyResult<()> {
        // Parameter Check and Correction
        let max_angle = if servo_id == 2 { 110 } else { 180 };
        let clamped_angle = angle.clamp(0, max_angle);


        // When parameter correction occurs, output a warning
        if angle != clamped_angle {
            println!("Warning: Angle for servo {} adjusted from {} to {}",
                servo_id, angle, clamped_angle);
        }


        // Convert to the actual angle value to send
        let actual_angle = clamped_angle as u8;


        let servo_command = [SERVO_ADDRESS, servo_id, actual_angle];
        self.i2c.write(&servo_command)
            .map_err(|e| PyOSError::new_err(e.to_string()))?;
        
        println!("Servo {} set to angle: {}", servo_id, actual_angle);
        Ok(())
    }

    #[pyo3(signature = (state, color = 0))]
    pub fn Ctrl_WQ2812_ALL(&mut self, state: i32, color: u8) -> PyResult<()> {
        // State Parameter Check
        let clamped_state = state.clamp(0, 1);
        if state != clamped_state {
            println!("Invalid state {}, adjusted to {}", state, clamped_state);
        }

        // Construct Command Based on State
        let led_command = if clamped_state == 1 {
            [LED_ADDRESS, 0x01, color]  // Open Command (with color parameter)
        } else {
            [LED_ADDRESS, 0x00, 0x00]   // Close Command
        };


        self.i2c.write(&led_command)
            .map_err(|e| PyOSError::new_err(e.to_string()))?;

        // State Output
        match clamped_state {
            1 => println!("LED on, color: {}", color),
            _ => println!("LED off")
        }

        Ok(())
    }

    #[pyo3(signature = (unit_id, state, color = 0))]
    pub fn Ctrl_WQ2812_Unit(&mut self, unit_id: u8, state: i32, color: u8) -> PyResult<()> {
        // State Parameter Check
        let clamped_state = state.clamp(0, 1);
        if state != clamped_state {
            println!("Invalid state {} for unit {}, adjusted to {}",

                state, unit_id, clamped_state);
        }

        // Construct Command
        let led_command = if clamped_state == 1 {
            [LED_UNIT_ADDRESS, unit_id, 0x01, color]  // Open Command

        } else {
            [LED_UNIT_ADDRESS, unit_id, 0x00, 0x00]   // Close Command
        };


        self.i2c.write(&led_command)
            .map_err(|e| PyOSError::new_err(e.to_string()))?;

        // State Output

        match clamped_state {
            1 => println!("LED unit {} on, color: {}", unit_id, color),
            _ => println!("LED unit {} off", unit_id)
        }

        Ok(())
    }

    #[pyo3(signature = (state))]
    pub fn Ctrl_Ultrasonic_Switch(&mut self, state: i32) -> PyResult<()> {
        // State Parameter Check and Correction
        let clamped_state = state.clamp(0, 1);
        if state != clamped_state {
            println!("Invalid state {}, adjusted to {}", state, clamped_state);
        }

        // Construct Control Command
        let command = [ULTRASONIC_SENSOR_ADDRESS, clamped_state as u8];
        

        self.i2c.write(&command)
            .map_err(|e| PyOSError::new_err(e.to_string()))?;

        // Output Status Information
        match clamped_state {
            1 => println!("Ultrasonic sensor enabled"),
            _ => println!("Ultrasonic sensor disabled")
        }


        Ok(())
    }
}

#[pymodule]
fn vehicle(_py: Python, m: &PyModule) -> PyResult<()> {
    m.add_class::<Vehicle>()?;
    m.add_class::<vehicle_controller::VehicleController>()?;
    Ok(())
}